#include <dispatch_system/mecanum_vehicle.h>
#include <dispatch_system/moying_map.h>
#include <schedule_config/config.hpp>

namespace moying
{
namespace navigation
{
    MecanumVehicle::MecanumVehicle()
    {

    }
    
    MecanumVehicle::MecanumVehicle(std::string type, const std::string &name)
    :Vehicle(type, name)
    {

    }

    MecanumVehicle::MecanumVehicle(std::string type, const std::string &name, unsigned int id)
    :Vehicle(type, name, id)
    {

    }

    MecanumVehicle::~MecanumVehicle()
    {

    }

    bool MecanumVehicle::isOmniVehicle(){
        return true;
    }


    void MecanumVehicle::setMovePatterns()
    {

        auto cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
        int search_theta_num = cfg_info.traffic_manager_cfg.search_theta_num;
        if(search_theta_num != 8 && search_theta_num != 4){
            ERROR("search_theta_num param is invalid, value:"<<search_theta_num);
            return;
        }

        move_patterns_.clear();
        
        // 麦轮状态机
        // 0: 静止时可执行的动作
        //                               x, y, th, t, cost, sm_state
        move_patterns_[0] = std::make_shared<MovePatterns>();

        move_patterns_[0]->push_back(kAcceleratePattern);          // 匀加速
        // move_patterns_[0]->push_back(kMoveForwardStopPattern);     // 前进停止
        move_patterns_[0]->push_back(kReduceSpeedPattern);     // 前进停止
        move_patterns_[0]->push_back(kWaitPattern);                 // 等待
        move_patterns_[0]->push_back(kRotateLeft45Pattern);         // 左转45
        move_patterns_[0]->push_back(kRotateLeft90Pattern);         // 左转90
        move_patterns_[0]->push_back(kRotateLeft135Pattern);        // 左转135
        move_patterns_[0]->push_back(kRotate180Pattern);            // 左转180
        move_patterns_[0]->push_back(kRotateR45Pattern);            // 右转45
        move_patterns_[0]->push_back(kRotateR90Pattern);            // 右转90
        move_patterns_[0]->push_back(kRotateR135Pattern);           // 右转135
        move_patterns_[0]->push_back(kLeftTransAccePattern);        // 左平移加速
        move_patterns_[0]->push_back(kRightTransAccePattern);       // 右平移加速
        move_patterns_[0]->push_back(kLeftTransReducePattern);      // 左平移减速
        move_patterns_[0]->push_back(kRightTransReducePattern);     // 右平移减速
        move_patterns_[0]->push_back(kReduceSpeedBackPattern);         // 减速后退
        move_patterns_[0]->push_back(kAccelerateBackPattern);          // 加速后退
        
        // 1: 匀速时可执行的动作
        //                               x, y, th, t, cost, sm_state
        move_patterns_[1] = std::make_shared<MovePatterns>();
        move_patterns_[1]->push_back(kMoveForwardPattern); // 匀速
        move_patterns_[1]->push_back(kReduceSpeedPattern); // 匀减速
        move_patterns_[1]->push_back(kTurnLeftPattern);     // 左转弯
        move_patterns_[1]->push_back(kTurnRightPattern);    // 右转弯
        move_patterns_[1]->push_back(kLeftFrontPattern);     // 左前方
        move_patterns_[1]->push_back(kRightFrontPattern);    // 右前方

        //左平移时可执行的动作
        move_patterns_[2] = std::make_shared<MovePatterns>();
        move_patterns_[2]->push_back(kLeftTranslationPattern);            // 左平移
        move_patterns_[2]->push_back(kLeftTransReducePattern);            // 左平移减速

        //右平移时可执行的动作
        move_patterns_[3] = std::make_shared<MovePatterns>();
        move_patterns_[3]->push_back(kRightTranslationPattern);            // 右平移
        move_patterns_[3]->push_back(kRightTransReducePattern);            // 右平移减速

        //后退时可执行的动作
        move_patterns_[4] = std::make_shared<MovePatterns>();
        move_patterns_[4]->push_back(kMoveBackwardPattern);            // 匀速后退
        move_patterns_[4]->push_back(kReduceSpeedBackPattern);         // 减速后退
        move_patterns_[4]->push_back(kAccelerateBackPattern);          // 加速后退
        
        HIGHLIGHT("omni vehicle set move patterns");
    }

    double MecanumVehicle::getTheta(int dir)
    {
        switch(dir)
        {
            case 0:
                return 0.0;
            case 1:
                return M_PI/4.0;
            case 2:
                return M_PI/2.0;
            case 3:
                return M_PI*3.0/4.0;
            case 4:
                return -M_PI;
            case 5:
                return -M_PI*3.0/4.0;
            case 6:
                return -M_PI/2.0;
            case 7:
                return -M_PI/4.0;
            default:
                ERROR("Unknown theta direction "<<dir);
                return 0;
        }
        return 0;
    }

    void MecanumVehicle::getNearNodes(const VehicleState &state, std::vector<Node> &nodes)
    {
        
    }

    bool MecanumVehicle::getPathFootprintOccupiedCells(MoyingMapPtr moyingmap, const NodePtr &a, const NodePtr &b, std::map<int, FootprintCell> &footprint_cells)
    {

        
    }

    void MecanumVehicle::constructPlanFromGraphPath(MoyingMapPtr moyingmap, const VehicleState &start, const VehicleState &goal, 
                    const std::vector<Node> &node_path, PlanningResult &result)
    {

        if(moyingmap == nullptr)
            return;
        result.node_path.clear();
        result.path.clear();
// int eclapse = 0;
        if(node_path.size() > 0)
        {
            auto cci_vec = moyingmap->corrected_cells(this->getID());
            bool first_node_handled = false;
            float theta = 0;
            float velocity = 0;
            int start_mx,start_my,goal_mx,goal_my,start_theta;
            moyingmap->getCostMap()->worldToMap(start.x,start.y,start_mx,start_my);
            moyingmap->getCostMap()->worldToMap(goal.x,goal.y,goal_mx,goal_my);
            start_theta = getCloseTheta(start.th);
            auto last_state = start;
            Node last_node = node_path.front();
            int segment_cell_num = 0;
            bool check_segment_begin = false;

            for(int i = 0; i < node_path.size(); i++)
            {
                auto node = node_path[i];
                Node next_node;
                if(i+1 < node_path.size()){
                    next_node = node_path[i+1];
                }else{
                    next_node = node_path[node_path.size()];
                }
                int x = node.x;
                int y = node.y;
                if(result.path.size()>0){
                    last_state = result.path.back();
                    last_node = result.node_path.back();
                }

                float cell_theta    = getTheta(node.th);
                cell_theta = node.theta == 0 ? cell_theta : node.theta;
                double cell_nav_x,cell_nav_y;
                if( cci_vec != nullptr &&  (*cci_vec)[x][y].cell_x != -1 ){   //使用更改过的导航坐标
                    int raw_map_x = (*cci_vec)[x][y].raw_map_x;
                    int raw_map_y = (*cci_vec)[x][y].raw_map_y;
                    moyingmap->getCostMap()->rawMapToWorld(raw_map_x, raw_map_y, cell_nav_x, cell_nav_y);

                }else{
                    moyingmap->getCostMap()->mapToWorld(x, y, cell_nav_x, cell_nav_y);
                }
                
                float theta_dis = fabs(cell_theta - last_state.th); //上一点与当前点的角度差
                if(theta_dis > M_PI){
                    theta_dis = M_PI*2-theta_dis; 
                }

                if(start_mx == x && start_my == y){

                    if(goal_mx == x && goal_my == y){//起始点和终止点是同一个
                        
                        if(node.sm_state == kMoveForwardState || fabs(cell_theta-goal.th) < SAME_POINT_COOR_DIS){   //前进移动模式或者朝向和终点cell一致,设成终点坐标 
                            cell_nav_x = goal.x;
                            cell_nav_y = goal.y;
                        }else{
                            cell_nav_x = start.x;
                            cell_nav_y = start.y;
                        }

                    }else{
                        cell_nav_x = start.x;
                        cell_nav_y = start.y;
                    }
                    
                }else if(goal_mx == x && goal_my == y){
                    cell_nav_x = goal.x;
                    cell_nav_y = goal.y;
                }

                VehicleState state;
                state.x = cell_nav_x;
                state.y = cell_nav_y;
                state.th = cell_theta;
                // state.t = result.start_time + std::chrono::milliseconds(1000 * node.t);
                state.t = result.start_time + std::chrono::milliseconds(int(moyingmap->time_resolution()*1000 * node.t));

                if( result.node_path.size()>0 ){
                    // auto last_node = result.node_path.back();
                    // bool move_forward = (node.x != last_node.x || node.y != last_node.y);
                    bool move_forward = fabs(state.x - last_state.x) > SAME_POINT_COOR_DIS 
                                        || fabs(state.y - last_state.y) > SAME_POINT_COOR_DIS;
                    if(move_forward){
                        float velocity = getVelocity(kMoveForwardState);
                        velocity = velocity == 0 ? 0.5 : velocity;
                        switch(node.sm_state){
                            case kStaticState:
                            {
                                if(last_node.x == node.x && (node.th == 0 || node.th == 4)){
                                    if(node.th == 0){
                                        if(node.y > last_node.y)
                                            state.move_mode = 2;
                                        else
                                            state.move_mode = 3;
                                    }else{
                                        if(node.y > last_node.y)
                                            state.move_mode = 3;
                                        else
                                            state.move_mode = 2;
                                    }
                                    
                                }else if(last_node.y == node.y && (node.th == 2 || node.th == 6)){
                                    if(node.th == 6){
                                        if(node.x > last_node.x)
                                            state.move_mode = 2;
                                        else
                                            state.move_mode = 3;
                                    }else{
                                        if(node.x > last_node.x)
                                            state.move_mode = 3;
                                        else
                                            state.move_mode = 2;
                                    }
                                }else{
                                    state.move_mode = 0;
                                }
                            }
                            break;
                            case kMoveForwardState:
                            {
                                state.vel_x = cos(cell_theta)*velocity;
                                state.vel_y = sin(cell_theta)*velocity;
                                state.move_mode = 0;
                            }
                            break;
                            case kMoveBackwardState:
                            {
                                state.vel_x = -cos(cell_theta)*velocity;
                                state.vel_y = -sin(cell_theta)*velocity;
                                state.move_mode = 4;
                            }
                            break;
                            case kLeftMoveShiftState:
                            {
                                state.vel_x = sin(cell_theta)*velocity;
                                state.vel_y = cos(cell_theta)*velocity;
                                state.vel_x = -state.vel_x;
                                state.move_mode = 2;
                            }
                            break;
                            case kRightMoveShiftState:
                            {
                                state.vel_x = sin(cell_theta)*velocity;
                                state.vel_y = cos(cell_theta)*velocity;
                                state.vel_y = -state.vel_y;
                                state.move_mode = 3;
                            }
                            break;
                        }

                        if(i==node_path.size()-1){
                            state.vel_x = 0;
                            state.vel_y = 0;
                            state.vel_th=0;
                        }
                        
                    }else{
                        state.move_mode = 1;
                    }
                }
                
                result.path.push_back(state);
                result.node_path.push_back(node);
                
                //直线行驶时根据时间更新到达点坐标
            }

            //如果最后一个node的朝向和终点朝向差超过一定的距离,加一个旋转
            if( !result.path.empty() && fabs(result.path.back().th-goal.th) > SAME_POINT_THETA_DIS){

                auto end = result.node_path.back();
                WARNING("last node x:"<<end.x<<" y:"<<end.y<<" th:"<<end.th
                <<" t:"<<end.t<<" end_path_theta:"<<result.path.back().th<<" goal_th:"<<goal.th);

                Node last_node = result.node_path.back();
                last_node.t = result.node_path.back().t + 2;
                last_node.sm_state = kStaticState;
                VehicleState last_state = goal;
                last_state.move_mode = 1;
                last_state.t = result.path.back().t + std::chrono::seconds(2);
                result.node_path.push_back(last_node);
                result.path.push_back(last_state);
            }
        }
        else 
        {
            WARNING("path size is zero");
        }

// WARNING("============equalizePathPoint eclapse:"<<eclapse);

        result.start = start;
        if(result.path.empty()){
            WARNING("result path size is empty");
            return ;
        }
        result.path.back().vel_th = 0;
        result.path.back().vel_x  = 0;
        result.path.back().vel_y  = 0;
        result.goal = result.path[result.path.size() - 1];
    }

    bool MecanumVehicle::getLineSegmentOfNodePath(const std::vector<Node> &nodes,int start_index,int& end_index){

        int segment = 0;
        float theta = getTheta(nodes[start_index].th);
        if(nodes[start_index].theta != 0){
            theta = nodes[start_index].theta;
        }

        for(int i=start_index;i<nodes.size();i++){
            int n = i+1;
            end_index = i;
            if(n >= nodes.size())
                break;
            if(nodes[n].x != nodes[i].x || nodes[n].y != nodes[i].y){   
                float theta2 = getTheta(nodes[i].th);
                if(nodes[i].theta != 0){
                    theta2 = nodes[i].theta;
                }
                // if(theta2 != theta && (nodes.back().x != nodes[n].x || nodes.back().y != nodes[n].y)){  //下一个cell的朝向发生了变化,且下一个cell不是目标点cell
                if(theta2 != theta){  //下一个cell的朝向发生了变化,且下一个cell不是目标点cell
                    break;
                }else{  //下一个cell朝向没变,线段加1
                    segment++;
                }
            }
        }

        if(segment < 2)
            return false;
        else{
            printf("segment start_index:%d,end_index:%d\n",start_index,end_index);
            return true;
        }
        
    }

    bool MecanumVehicle::equalizePathPoint(MoyingMapPtr moyingmap,const std::vector<Node>& nodes,
            std::chrono::system_clock::time_point start_time,VehicleState start_state,VehicleState goal_state,int start_index,int end_index,
            std::vector<VehicleState> &path,int& collide_index){

        // WARNING("equalizePathPoint start_index:"<<start_index<<" end_index:"<<end_index);
        // const std::vector<Node>& nodes = result.node_path;
        if(start_index < 0 || start_index >= nodes.size() || end_index < 0 || end_index >= nodes.size() || nodes.empty())
            return false;

        Node start  = nodes[start_index];
        Node end    = nodes[end_index];
        Node g_start  = nodes[0];
        Node g_end    = nodes[nodes.size()-1];

        int segment_num = 0;
        Node last_node = start;
        for(int i=start_index;i<=end_index;i++){
            if(last_node.x == nodes[i].x && last_node.y == nodes[i].y )
                continue;
            segment_num++;
            last_node = nodes[i];
        }

        if(segment_num < 2){
            WARNING("segment number too less, number:"<<segment_num);
            return false;
        }

        double x1,y1,x2,y2;
        if( !moyingmap->getCellNavCoor(start.x,start.y,x1,y1,getID()) )
            return false;
        if( !moyingmap->getCellNavCoor(end.x,end.y,x2,y2,getID()) )
            return false;

        if(start.isEqual(g_start)){
            x1 = start_state.x;
            y1 = start_state.y;
        }
        if(end.isEqual(g_end)){
            x2 = goal_state.x;
            y2 = goal_state.y;
        }

        double x_delta = fabs(x1-x2);
        double y_delta = fabs(y1-y2);
        float dis = sqrt(x_delta*x_delta + y_delta*y_delta); 
        float dis_inc   = dis/segment_num;
        float x_inc     = x_delta / segment_num;
        float y_inc     = y_delta / segment_num;
        x_inc = x2>x1 ? x_inc : -x_inc;
        y_inc = y2>y1 ? y_inc : -y_inc;

        if(dis == 0)
            return false;

        float theta = 0;
        float cos_value = (x2-x1)/dis;
        cos_value = cos_value < -1 ? -1 : cos_value;
        cos_value = cos_value > 1 ? 1 : cos_value;
        theta = acos(cos_value);
        if(y2 < y1)
            theta = -theta;

        last_node = start;
        VehicleState state = start_state;
        
        sch::ConfigInfo cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();

        unsigned char start_cost = moyingmap->getCostMap()->getCost(start.x,start.y);
        float len = length_ + cfg_info.traffic_manager_cfg.robot_length_padding+0.2;
        float wid = width_ + cfg_info.traffic_manager_cfg.robot_width_padding+0.3;
        int segment = 0;
        for(int i=start_index+1;i<=end_index;i++){

            unsigned char cur_cost = moyingmap->getCostMap()->getCost(nodes[i].x,nodes[i].y);
            if(start_cost != cur_cost){//这里保证只在连续相同类型的cell里做平直处理
                collide_index = i;
                return false;
            }

            if( last_node.x == nodes[i].x && last_node.y == nodes[i].y ){
                state.t = start_time + std::chrono::milliseconds(int(moyingmap->time_resolution()*1000 * nodes[i].t));
                state.move_mode = 1;
                if(nodes[i].x == end.x && nodes[i].y == end.y){ //最后一个cell中的theta和node保持一致
                    float theta2 = nodes[i].theta;
                    state.th = theta2 == 0 ? getTheta(nodes[i].th) : theta2;
                    // path.emplace_back(state);
                    // if(i != end_index){ //只保留最后一个cell的第一个旋转和最后一个旋转
                    //     float theta3 = nodes[end_index].theta;
                    //     state.t = start_time + std::chrono::milliseconds(1000 * nodes[end_index].t);
                    //     state.th = theta3 == 0 ? getTheta(nodes[end_index].th) : theta3;
                    //     path.emplace_back(state);
                    //     return true;
                    // }
                }
                path.emplace_back(state);
                continue;
            }

            segment++;
            state.move_mode = 0;
            state.th = theta;
            // state.t = start_time + std::chrono::milliseconds(1000 * nodes[i].t);
            state.t = start_time + std::chrono::milliseconds(int(moyingmap->time_resolution()*1000 * nodes[i].t));
            state.x = x1 + x_inc*segment;
            state.y = y1 + y_inc*segment;

            if(segment < segment_num){  
                int t_dis = nodes[i].t-last_node.t;
                if(t_dis <= 0)
                    return false;
                float velocity = dis_inc / (t_dis*moyingmap->time_resolution());
                state.vel_x = cos(state.th)*velocity;
                state.vel_y = sin(state.th)*velocity;
                // WARNING("dis:"<<dis<<" dis_inc:"<<dis_inc<<" segment_num:"<<segment_num<<" velocity:"<<velocity<<" t_dis:"<<t_dis);
            }else{
                state.vel_x = state.vel_y = 0;//最后一个点的速度为0,不需要设
            }


            // if(nodes[i].th == 1 || nodes[i].th == 3 || nodes[i].th == 5 || nodes[i].th == 7 ){
            //     wid += 0.2;
            // }

            // static int times=0;
            // static int elapse = 0;
            // auto start_time = std::chrono::system_clock::now();
            
            double owx,owy;
            if( !moyingmap->getCellNavCoor(nodes[i].x,nodes[i].x,owx,owy,getID()) )
                return false;
            double x_dis = fabs(owx-state.x);
            double y_dis = fabs(owy-state.y);
            if(x_dis+y_dis > 0.02)//当点和原来的位置偏移过大时才检测碰撞
            { 
                if(moyingmap->checkCollision(len,wid,state.x,state.y,state.th,this)){

                    // WARNING("equalize path point collide,cell_x:"<<nodes[i].x<<" cell_y:"<<nodes[i].y<<" state.x:"<<state.x
                    //     <<" state.y:"<<state.y<<" state.th"<<state.th);

                    // elapse += std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start_time).count(); 
                    // printf("====times:%d elapse:%d\n",++times,elapse);
                    
                    collide_index = i;
                    return false;
                }
                // elapse += std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start_time).count(); 
                // printf("====times:%d elapse:%d\n",++times,elapse);
            }

            path.emplace_back(state);

            last_node = nodes[i];
        }

        //把最后一个点的theta更新为目标点theta
        if(end_index == nodes.size()-1){
            float theta_dis = fabs(path.back().th - goal_state.th);
            if( theta_dis > SAME_POINT_THETA_DIS && theta_dis < 0.785){
                path.back().th = goal_state.th;
            }
        }

        return true;
    }

   

    unsigned int MecanumVehicle::getStartNodeIndex(const VehicleState &start_state, char &sm)
    {
        
    }

    unsigned int MecanumVehicle::getGoalNodeIndex(const VehicleState &start_state, char &sm)
    {

    }
}
}